clear;
close all;
clc;
rad=0.2
thre = 0.2;
% thre = 0.35;
% freqs= [300 600 1500 3000]; %just a random frequency
% freqs= [850 1000]; %just a random frequency
freqs= 100:100:3000; %just a random frequency
fs = 44100;
% whos
plot_each =0;
tdur=40e-3;
projection1 = 0; % neariest center
% projection1 = 1; % first project to the unit circle
Num_centers = 1;
Proto_Num = 2;

Angles = 10:10:90;
% Angles = 10:10:80;
conds = 'AB';
sides = {'-','+'};

PString = num2str(Proto_Num);
if Proto_Num==2.4
    PString = [num2str(Proto_Num) '0'];
end
for iangle = 1:length(Angles)
    iangle
    cc = 0;
    if plot_each==1
        f=figure('position',[100 70 900 500]);
    end
    for icond = 1:2
        [recorded_sound,fs]=audioread(['C:\Users\gaiy\OneDrive - Saint Louis University\Sound_loc\3Dio recordings\Protocol_' PString '_deg' sides{icond} num2str(abs(Angles(iangle))) '.mp3']);
        recorded_sound=resample(recorded_sound(fs:end-fs,:),441,320);
        left_sound = recorded_sound(:,1);
        right_sound = recorded_sound(:,2);
        for ifreql = 1:length(freqs)
            freq = freqs(ifreql);
            legendall{ifreql}=[num2str(freq) ' Hz'];
            [SL,SR,freq_pos,flag]=mySTFT(left_sound,right_sound,fs,thre,tdur);
            %             freq_pos(1:100)'
            % normalization:
            [SL1,SR1]=Normalize(SL,SR);
            [Y Ind] = min(abs(freq_pos - freq));
            X_P = [real(SR1(Ind,flag>0))' imag(SR1(Ind,flag>0))'];

            % %             X_P = f_cleanup_Bnoise(X_P,fs,freq,tdur);
            cc=cc+1;
            if plot_each==1
                figure(f)
                subplot(5,7,cc)
                for i=1:length(X_P)
                    plot(X_P(i,1),X_P(i,2),'k.');hold on;
                end
                deg_sign=char(0176);
                %                         [IDX, C] = kmeans(X_P, Num_centers,'Distance','cityblock','Replicates',5);
            end
            out = f_mycenter(X_P,rad);
            C = out.C1;
            %             eval(['load Freq_centers' num2str(freq) ])
            % eval(['load Freq_centers_hardware' num2str(freq) ])
            eval(['load Model_centers\Freq_centers_sinu' num2str(freq) ])

            if plot_each==1
                for i=1:length(F_centers)
                    s=plot(F_centers(i,1),F_centers(i,2),'go');hold on;set(s,'linewidth',2)
                end
                ss=text(0.6,-0.8,[num2str(freq) ' Hz']);set(ss,'fontsize',14,'color','r')

                plot(C(1,1),C(1,2),'bo','Markersize',20,'LineWidth',2);hold on;
                grid on
                set(gca,'Xtick',-1:0.5:1)
                set(gca,'Ytick',-1:0.5:1)
                axis([-1 1 -1 1])
                hold on;
                plot(-1:1,[0,0,0],'k', 'LineWidth', 0.5);
                plot([0,0,0],-1:1,'k','LineWidth',0.5)
                set(gca,'fontsize',10)
                xlabel('Re','fontsize',12)
                ylabel('Im','fontsize',12)
                hold on
                % now plot the line:
                C_new = C./sqrt(C(1)^2+C(2)^2);
                plot([0 C_new(1)],[0 C_new(2)],'r');
                plot(x_spiral,y_spiral,'color',[0.5 0.5 0.5]) ; % nturns crossings, including end point
                axis square
                pause(0.1)
            end

            %             out = f_angle_class_center(C,F_centers,projection1); % cluster centers
            out = f_spiral_classification(C,x_spiral,y_spiral);
            fine_angles = linspace(-90, 90, length(x_spiral));

            %             classified_angle= true_angle(out.class)
            classified_angle= fine_angles(out.class);
            record_class{icond}(ifreql,iangle)=classified_angle;
            if plot_each==1
                if icond==1 & ifreql == 1
                    t = text(-2.8, 0.5, ['True Loc: ' num2str(-Angles(iangle)) ])
                elseif icond==2 & ifreql == 1
                    t = text(-2.8, 0.5, ['True Loc: ' num2str(Angles(iangle)) ])
                end
                set(t,'fontsize',12, 'FontWeight','bold')
                title(['Classified Loc:' num2str(round(classified_angle))])
            end
            X_sp_freq(ifreql,:)=x_spiral;
            y_sp_freq(ifreql,:)=y_spiral;
            C_freq(ifreql,:)=C;
        end % freq
        BegF = 1;
        EndF=30;
        outMul = f_spiral_classification_MultiF(C_freq(BegF:EndF),X_sp_freq(BegF:EndF,:),y_sp_freq(BegF:EndF,:));
        fine_angles = linspace(-90, 90, length(x_spiral));

        classified_MultiF= fine_angles(outMul.class)
        record_classM{icond}(iangle)=classified_MultiF;

        pause(0.01)
    end
end


figure('position',[50 70 1800 900]);
% subplot(1,2,1)
% pc = 'bmg';
for ifreq = 1:length(freqs)
    subplot(4,8,ifreq)
    s=plot([fliplr(-Angles) Angles],[fliplr(record_class{1}(ifreq,:)) record_class{2}(ifreq,:)]');hold on;
    set(s,'linewidth',2)
    title([num2str(freqs(ifreq)) 'Hz'])

    s=plot([-100 100],[-100 100],'k');set(s,'linewidth',1)
    xlabel('True (deg)','fontsize',10)
    ylabel('classified (deg)','fontsize',10)
    axis square

end

% subplot(1,2,2)
figure
s=plot([fliplr(-Angles) Angles],[fliplr(record_classM{1}) record_classM{2}]','r');hold on;
set(s,'linewidth',2)

s=plot([-100 100],[-100 100],'k');set(s,'linewidth',1)
legend('Combined Freqs','Location','northwest')
legend boxoff
xlabel('True Angle (degrees)','fontsize',12)
ylabel('classified Angle (degrees)','fontsize',12)
title('Single Source')
axis square
